﻿using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;
using SpaceBattle.Model;

namespace SpaceBattle.Utilities
{
    static class PhysicsCalculator
    {
        private const float Gravity = 5f;

        public static float Distance(Vector2 position1, Vector2 position2)
        {
            return Vector2.Distance(position1, position2);
        }

        private static float GravityForce(Body source, Body body)
        {
            double distance = Distance(source.Position, body.Position);

            return Gravity * (source.Mass * body.Mass) / (float)Math.Pow(distance, 2) / body.Mass;
        }

        public static Vector2 GravityVector(Body source, Body body)
        {
            float gravity = GravityForce(source, body);

            Vector2 normalizedDir = new Vector2(source.Position.X - body.Position.X, source.Position.Y - body.Position.Y);

            normalizedDir.Normalize();

            return normalizedDir * gravity;
        }

        public static void countForces(List<Body> PhysicBodies)
        {
            Vector2 partialRes;

            foreach (var body in PhysicBodies)
            {
                if (!body.Solid)
                {
                    partialRes = Vector2.Zero;

                    foreach (var source in PhysicBodies)
                    {
                        if (source != body)
                        {
                            partialRes += PhysicsCalculator.GravityVector(source, body);
                            if (Distance(source.Position, body.Position) < (source.Size + body.Size) / 2)
                            {
                                Vector2 vector = new Vector2(body.Position.X - source.Position.X, body.Position.Y - source.Position.Y);
                                vector.Normalize();
                                body.Force += vector * ((source.Size + body.Size) / 2 - Distance(source.Position, body.Position)) / 5;
                            }
                        }
                    }

                    body.Force += partialRes;
                }
            }

            UpdateShip(PhysicBodies[1]);
        }

        private static void UpdateShip(Body ship)
        {
            //(float)(-Math.Sin((GameManager.playersCar.cannon.Rotation - 90) * Math.PI / 180) * 5 * 1000), 
            //(float)(Math.Cos((GameManager.playersCar.cannon.Rotation - 90) * Math.PI / 180) * 5 * 1000)

            if (Input.IsKeyDown(Keys.Up))
            {
                float xForce = -(float)Math.Sin((ship.Rotation + 90) * Math.PI / 180) * .1f;
                float yForce = (float)Math.Cos((ship.Rotation + 90) * Math.PI / 180) * .1f;

                ship.Force += new Vector2(xForce, yForce);
            }
            if (Input.IsKeyDown(Keys.Down))
            {
                float xForce = -(float)Math.Sin((ship.Rotation + 90) * Math.PI / 180) * .1f;
                float yForce = (float)Math.Cos((ship.Rotation + 90) * Math.PI / 180) * .1f;

                ship.Force -= new Vector2(xForce, yForce);
            }
            if (Input.IsKeyDown(Keys.Left))
            {
                ship.Rotation -= 3;
            }
            if (Input.IsKeyDown(Keys.Right))
            {
                ship.Rotation += 3;
            }

        }
    }

    public class Body : GameComponent
    {
        private float mass;
        private Vector2 position;
        private Vector2 force;
        private Boolean solid;
        private float rotation;
        private float size;
        private Color color;

        public Color Color
        {
            get { return color; }
        }

        public float Size
        {
            get { return size; }
        }

        public float Rotation
        {
            get
            {
                return MathHelper.ToDegrees(rotation);
            }
            set
            {
                rotation = MathHelper.ToRadians(value) % (float)(2 * Math.PI);
            }
        }

        public Boolean Solid
        {
            get { return solid; }
            set { solid = value; }
        }

        public Vector2 Force
        {
            get { return force; }
            set { force = value; }
        }

        public Vector2 Position
        {
            get { return position; }
            set { position = value; }
        }

        public float Mass
        {
            get { return mass; }
        }

        public Body(float mass, Vector2 position, float size, Color color)
            : base(GameMain.Game())
        {
            GameMain.Game().Components.Add(this);
            this.mass = mass;
            this.position = position;
            this.size = size;
            this.color = color;
        }

        public override void Update(GameTime gameTime)
        {
            float upperBound = 5000;
            float lowerBound = -5000;

            Position += force;
            if (Position.X > upperBound)
                Position = new Vector2(lowerBound,Position.Y);
            else if (Position.X < lowerBound)
                Position = new Vector2(upperBound, Position.Y);
            if (Position.Y > upperBound)
                Position = new Vector2(Position.X, lowerBound);
            else if (Position.Y < lowerBound)
                Position = new Vector2(Position.X, upperBound);
        }

    }
}
